/**
 ******************************************************************************
 * @file    BR_ChassisMotion.c
 * @author  FSF
 * @date    2019/12/30
 * @brief   舵轮底盘运动学
 ******************************************************************************
 * @note    轮子序号定位: 0,1,2,3 对应右前, 右后, 左前, 左后
 ******************************************************************************
 */
#include "BR_Common.h"
#include "arm_math.h"
#include "stdint.h"

static const int8_t prvSelfTurnVelComponentTbl[4][2] = {{-1,1},{1,1},{-1,-1},{1,-1}};  // 用于求每个轮子自转时的速度分量
static float prvChassisVelCloseLoopCorrect(float TargetVel);
static void prvStrangeConditionJudge(float *Vel, float *Pos, int MotorId);
/* -------------------------------------------------- Public ----------------------------------------------- */
/**
 * @brief   底盘运动控制,输入速度、方向、自转角速度,向电机发送控制指令
 * @param   Vel  : 速度大小[mm/s] (大于等于0)
            Dir  : 世界坐标系下的方向[rad] -PI to PI
            Omega: 自转角速度[rad/s]
 */
void BR_vChassisMotionControl(float Vel, float Dir, float Omega)
{
    float PosMsgs[4], VelMsgs[4];   // Pos为rad, -PI~PI, Vel为 mm/s
    /* 底盘速度闭环 */
    if(BR_gRobot.StateFlags.eControlMode == AUTO)  // 速度闭环仅在自动模式下启用,
        Vel = prvChassisVelCloseLoopCorrect(Vel);
    /* 将Dir转至机器人坐标系Dir_ */
    float Dir_ = Dir - BR_gRobot.ChassisState.Theta;
    Dir_ = ( Dir_ <= -PI ) ? ( 2 * PI + Dir_ ) : Dir_;
    Dir_ = ( Dir_ >= PI ) ? ( -2 * PI + Dir_ ) : Dir_;
    if(Vel != 0 || Omega != 0)      // 速度/角速度不为0的正常情况
    {
        /* 计算主运动速度x,y分量 */
        float VelX = -arm_sin_f32(Dir_) * Vel;
        float VelY = arm_cos_f32(Dir_) * Vel;
        /* 计算自转运动每个轮子的x,y分量，并与主速度分量合成 */
        float SelfTurnVel = Omega * BR_configCHASSIS_R;
        for(int i = 0; i < 4; i++)
        {
            float VelSumX = VelX + prvSelfTurnVelComponentTbl[i][0] * SelfTurnVel * SELF_TURN_VEL_X_K;  // 速度合成
            float VelSumY = VelY + prvSelfTurnVelComponentTbl[i][1] * SelfTurnVel * SELF_TURN_VEL_Y_K;
            VelMsgs[i] = COMPOUND(VelSumX, VelSumY);    // 和速度大小
            PosMsgs[i] = -atan2(VelSumX, VelSumY);      // 和速度方向
            prvStrangeConditionJudge(&VelMsgs[i], &PosMsgs[i], i);  // 当两帧间的方向差大于PI/2时, 执行特殊逻辑, 确保舵向走劣弧
        }
    }
    else  // 速度和角速度均为0，为了保证没有速度时舵向也会改变
    {
        for(int i = 0; i < 4; i++)
        {
            VelMsgs[i] = 0;
            PosMsgs[i] = Dir_;
            prvStrangeConditionJudge(&VelMsgs[i], &PosMsgs[i], i);
        }
    }
    /* 发送位置和速度指令 */
    BR_gRobot.ServoMotors.fServoMotorsPosSet(PosMsgs);
    BR_gRobot.DrivingMotors.fDrivingMotorsVelSet(VelMsgs);
}

/** @brief   底盘运动控制 单轮控制
 *  @param   WheelsVel 四轮速度信息[mm/s]
 *  @param   WheelsDir 四轮舵向信息（世界坐标系/自身坐标系）[rad] -PI～PI
 *  @note    单轮模式控制设计为用于舵轮底盘的精确定位
 *  @warning 不要在高速下启用该模式
 */
void BR_vWheelsControl(float *WheelsVel, float *WheelsDir)
{
    float PosMsgs[4], VelMsgs[4];
    for(int i = 0; i < 4; i++)
    {
        /* 将Dir转至机器人坐标系dir_(注释此区块则为自身坐标系，解注释则为世界坐标系) */
        // PosMsgs[i] = WheelsDir[i] - BR_gRobot.ChassisState.Theta;
        // PosMsgs[i] = ( PosMsgs[i] <= -PI ) ? ( 2 * PI + PosMsgs[i] ) : PosMsgs[i];
        // PosMsgs[i] = ( PosMsgs[i] >= PI ) ? ( -2 * PI + PosMsgs[i] ) : PosMsgs[i];
        PosMsgs[i] = WheelsDir[i];
        /* 直接使用四轮转速信息 */
        VelMsgs[i] = WheelsVel[i];
        prvStrangeConditionJudge(&VelMsgs[i], &PosMsgs[i], i);
    }
    /* 发送位置和速度指令 */
    BR_gRobot.ServoMotors.fServoMotorsPosSet(PosMsgs);
    BR_gRobot.DrivingMotors.fDrivingMotorsVelSet(VelMsgs);
}

/* -------------------------------------------------- Private ----------------------------------------------- */
/** @brief  私有函数, 在两帧的舵向差距在90°以上，270°以下时，可通过速度反向来保证走舵向劣弧
 *                              典型情况为从向前走瞬间切换为向后走，此时应舵向不动，速度反向
 *  @param   Vel     : 本帧速度
 *  @param   Pos     : 本帧位置
 *  @param   MotorId : 舵向电机ID */
static void prvStrangeConditionJudge(float *Vel, float *Pos, int MotorId)
{
    float ServoRelativePos = BR_gRobot.ServoMotors.RelativePos[MotorId];
    if(( *Pos - ServoRelativePos > PI / 2 && *Pos - ServoRelativePos < PI * 3 / 2 ) ||
       ( *Pos - ServoRelativePos < -PI / 2 && *Pos - ServoRelativePos > -PI * 3 / 2 ))  // 两帧间的位置差90°以上, 270°以下
    {
        *Pos = ( *Pos > 0 ) ? *Pos - PI : *Pos + PI;
        *Vel = -*Vel;
    }
}

/** @brief  车体自身的速度环修正，简单来说是使发到驱动器的速度略高于目标速度，来使实际速度接近目标速度
    @param  TargetVel 目标速度 (大于等于0)
    @return ActualSendVel 实际发送给驱动器的速度
 */
static float prvChassisVelCloseLoopCorrect(float TargetVel)
{
    float ActualSendVel, VelAddition;
    VelAddition = BR_configCHASSIS_VEL_CLOSELOOP_P * ( TargetVel - BR_gRobot.ChassisState.Vel );
    if( VelAddition >= 0 )  // 加速时的情况
    {
        float MaxAddition = BR_configCHASSIS_ACC_MAX_ADDITION_PARAM * TargetVel;                   // 以目标速度为基准限制最大的修正量
        VelAddition = VelAddition > MaxAddition ? MaxAddition : VelAddition;
    }
    else                    // 减速时的情况
    {
        float MaxAddition = BR_configCHASSIS_DEC_MAX_ADDITION_PARAM * BR_gRobot.ChassisState.Vel;  // 以当前速度为基准限制最大的修正量
        VelAddition = ( ABS(VelAddition) ) > MaxAddition ? -MaxAddition : VelAddition;
    }
    ActualSendVel = TargetVel + VelAddition;
    return ActualSendVel;
}
